#include "encode.h"
#include "zf_tim.h"
#include "zf_gpio.h"

Enco_type Enco;

void Enco_Init(void)
{
    Enco.Displace = 0;
    gpio_mode(P3_4, GPI_IMPEDANCE);
    ctimer_count_init(CTIM0_P34);
    gpio_mode(P5_0, GPI_IMPEDANCE);

    gpio_mode(P0_4, GPI_IMPEDANCE);
    ctimer_count_init(CTIM3_P04);
    gpio_mode(P5_2, GPI_IMPEDANCE);
}

void Enco_GetData(void)
{
    Enco.Left = ctimer_count_read(CTIM3_P04);
    Enco.Left *= LeftDire_Pin ? 1 : -1;
    ctimer_count_clean(CTIM3_P04);

    Enco.Right = ctimer_count_read(CTIM0_P34);
    Enco.Right *= RightDire_Pin ? -1 : 1;
    ctimer_count_clean(CTIM0_P34);

    // 每一时刻的Enco.Right + Enco.Left是速度，积分后是位移
    Enco.Velocity = Enco.Right + Enco.Left;
    Enco.Displace += Enco.Velocity;
}